/*
 * SogaFisica.cpp
 *
 *  Created on: Oct 7, 2013
 *      Author: Hernan
 */

#include "SogaFisica.h"
#include <iostream>
using namespace std;

SogaFisica::SogaFisica(b2World* world, float x, float y, float ancho,
		float alto, float angulo, string tipo) : EntidadFisica(tipo)  {
	this->anguloRotacion = angulo;


	this->definicionCuerpoFisico->position.Set(x, y);
	this->definicionCuerpoFisico->angle = anguloRotacion;
	this->cuerpoFisico = world->CreateBody(definicionCuerpoFisico);

	b2PolygonShape rectangulo;

	rectangulo.SetAsBox(alto / 2, ancho / 2);
	this->fixture = new b2FixtureDef();
	this->fixture->shape = &rectangulo;
	this->fixture->density = 2.0f;
	this->fixture->isSensor = true;
	this->cuerpoFisico->CreateFixture(this->fixture);

	this->guardarInformacionDeEntidad();
	this->jointCuerda = NULL;

	this->alto = alto;
	this->ancho = ancho;
}

void SogaFisica::jointear(b2World* world) {
	//elimino la soga de como estaba

	/*world->DestroyBody(this->cuerpoFisico);
	 delete this->definicionCuerpoFisico;
	 delete this->fixture;*/

	// Distance joint
//	float dampingRatio = 0.0;
//	float frequencyHz = 0;


	if (this->aJointearFinal->getTipo().compare("zpolea")!=0 && this->aJointearInicial->getTipo().compare("zpolea")!=0 ){

	cout<<"ENTRA EN EL JOINTEAR "<<endl;

	b2DistanceJointDef distJDef;
	b2Vec2 anchor1 = b2Vec2_zero;
	b2Vec2 anchor2 = b2Vec2_zero;
	b2Vec2 posicionJointearInicial = b2Vec2_zero;
	b2Vec2 posicionJointearFinal = b2Vec2_zero;

	if (this->aJointearInicial->getTipo().compare("balancin") == 0) {
		BalancinFisica* balancinFisica = (BalancinFisica*) aJointearInicial;
		if (aJointearInicial->entidadIzquierda == this) {
			posicionJointearInicial = balancinFisica->extremoizq->GetPosition();
			cout << "estoy en la izquierda 1" << endl;
			cout << "A jointer inicial en la izquierda: " << posicionJointearInicial.x << " "
					<< posicionJointearInicial.y << endl;
			cout << " y el centro es:  " << balancinFisica->obtenerPosicionX() << " " <<
					balancinFisica->obtenerPosicionY() << endl;
			anchor1.Set(-15, 0);

		}
		if (aJointearInicial->entidadDerecha == this) {
			posicionJointearInicial = balancinFisica->extremoder->GetPosition();
			cout << "estoy en la derecha 1" << endl;
			cout << "A jointer inicial en la derecha: " << posicionJointearInicial.x << " "
					<< posicionJointearInicial.y << endl;
			cout << " y el centro es:  " << balancinFisica->obtenerPosicionX() << " " <<
					balancinFisica->obtenerPosicionY() << endl;
			anchor1.Set(15, 0);
		}
	}
	if (this->aJointearFinal->getTipo().compare("balancin") == 0) {
		BalancinFisica* balancinFisica = (BalancinFisica*) aJointearFinal;

		if (aJointearFinal->entidadIzquierda == this) {
			posicionJointearFinal = balancinFisica->extremoizq->GetPosition();

			cout << "estoy en la izquierda 2" << endl;
			cout << "A jointer final en la izquierda: " << posicionJointearFinal.x << " "
					<< posicionJointearFinal.y << endl;
			cout << " y el centro es:  " << balancinFisica->obtenerPosicionX() << " " <<
					balancinFisica->obtenerPosicionY() << endl;
			anchor2.Set(-15, 0);

		}
		if (aJointearFinal->entidadDerecha == this) {
			cout << "estoy en la derecha 2" << endl;
			posicionJointearFinal = balancinFisica->extremoder->GetPosition();
			cout << "A jointer final en la derecha: " << posicionJointearFinal.x << " "
					<< posicionJointearFinal.y << endl;
			cout << " y el centro es:  " << balancinFisica->obtenerPosicionX() << " " <<
					balancinFisica->obtenerPosicionY() << endl;
			anchor2.Set(15, 0);

		}
	}

	// soga atada a tijera:

	if (this->aJointearInicial->getTipo().compare("atijera") == 0) {
		TijeraFisica* tijeraFisica = (TijeraFisica*) aJointearInicial;
		if (aJointearInicial->entidadIzquierda == this) {
			posicionJointearInicial = tijeraFisica->extremoizq->GetPosition();
			cout << "estoy en la izquierda 1" << endl;
			cout << "A jointer inicial en la izquierda: " << posicionJointearInicial.x << " "
					<< posicionJointearInicial.y << endl;
			cout << " y el centro es:  " << tijeraFisica->obtenerPosicionX() << " " <<
					tijeraFisica->obtenerPosicionY() << endl;
			anchor1.Set(4.5f, 0.0f);

		}
		if (aJointearInicial->entidadDerecha == this) {
			posicionJointearInicial = tijeraFisica->extremoder->GetPosition();
			cout << "estoy en la tijera derecha 1" << endl;
			cout << "A jointer inicial en la derecha: " << posicionJointearInicial.x << " "
					<< posicionJointearInicial.y << endl;
			cout << " y el centro es:  " << tijeraFisica->obtenerPosicionX() << " " <<
					tijeraFisica->obtenerPosicionY() << endl;
			anchor1.Set(-4.5f, 0.0f);
		}
	}
	if (this->aJointearFinal->getTipo().compare("atijera") == 0) {
		TijeraFisica* tijeraFisica = (TijeraFisica*) aJointearFinal;

		if (aJointearFinal->entidadIzquierda == this) {
			posicionJointearFinal = tijeraFisica->extremoizq->GetPosition();

			cout << "estoy en la izquierda 2" << endl;
			cout << "A jointer final en la izquierda: " << posicionJointearFinal.x << " "
					<< posicionJointearFinal.y << endl;
			cout << " y el centro es:  " << tijeraFisica->obtenerPosicionX() << " " <<
					tijeraFisica->obtenerPosicionY() << endl;
			anchor2.Set(4.5f, 0.0f);

		}
		if (aJointearFinal->entidadDerecha == this) {
			cout << "estoy en la tijera derecha 2" << endl;
			posicionJointearFinal = tijeraFisica->extremoder->GetPosition();
			cout << "A jointer final en la derecha: " << posicionJointearFinal.x << " "
					<< posicionJointearFinal.y << endl;
			cout << " y el centro es:  " << tijeraFisica->obtenerPosicionX() << " " <<
					tijeraFisica->obtenerPosicionY() << endl;
			anchor2.Set(-4.5f, 0.0f);

		}
	}


	if (this->aJointearInicial->getTipo().compare("carrito") == 0) {
		CarritoFisica* carritoFisica = (CarritoFisica*) aJointearInicial;
		if (aJointearInicial->entidadIzquierda == this) {
			posicionJointearInicial = carritoFisica->getExtremoIzquierdo()->GetPosition();
			cout << "estoy en la izquierda 1" << endl;
			cout << "A jointer inicial en la izquierda: " << posicionJointearInicial.x << " "
					<< posicionJointearInicial.y << endl;
			cout << " y el centro es:  " << carritoFisica->obtenerPosicionX() << " " <<
					carritoFisica->obtenerPosicionY() << endl;
			anchor1.Set(-7, 0);

		}
		if (aJointearInicial->entidadDerecha == this) {
			posicionJointearInicial = carritoFisica->getExtremoDerecho()->GetPosition();
			cout << "estoy en la tijera derecha 1" << endl;
			cout << "A jointer inicial en la derecha: " << posicionJointearInicial.x << " "
					<< posicionJointearInicial.y << endl;
			cout << " y el centro es:  " << carritoFisica->obtenerPosicionX() << " " <<
					carritoFisica->obtenerPosicionY() << endl;
			anchor1.Set(7, 0);
		}
	}
	if (this->aJointearFinal->getTipo().compare("carrito") == 0) {
		CarritoFisica* carritoFisica = (CarritoFisica*) aJointearFinal;

		if (aJointearFinal->entidadIzquierda == this) {
			posicionJointearFinal = carritoFisica->getExtremoIzquierdo()->GetPosition();

			cout << "estoy en la izquierda 2" << endl;
			cout << "A jointer final en la izquierda: " << posicionJointearFinal.x << " "
					<< posicionJointearFinal.y << endl;
			cout << " y el centro es:  " << carritoFisica->obtenerPosicionX() << " " <<
					carritoFisica->obtenerPosicionY() << endl;
			anchor2.Set(-7, 0);

		}
		if (aJointearFinal->entidadDerecha == this) {
			cout << "estoy en la tijera derecha 2" << endl;
			posicionJointearFinal = carritoFisica->getExtremoDerecho()->GetPosition();
			cout << "A jointer final en la derecha: " << posicionJointearFinal.x << " "
					<< posicionJointearFinal.y << endl;
			cout << " y el centro es:  " << carritoFisica->obtenerPosicionX() << " " <<
					carritoFisica->obtenerPosicionY() << endl;
			anchor2.Set(7, 0);

		}
	}


/******/

	if (this->aJointearInicial->getTipo().compare("arandela") == 0) {
		ArandelaFisica* arandelaFisica = (ArandelaFisica*) aJointearInicial;
		posicionJointearInicial = arandelaFisica->obtenerCuerpo()->GetPosition();
		anchor1.Set(-4, 0);
	}

	if (this->aJointearFinal->getTipo().compare("arandela") == 0) {
		ArandelaFisica* arandelaFisica = (ArandelaFisica*) aJointearFinal;
		posicionJointearFinal = arandelaFisica->obtenerCuerpo()->GetPosition();
		anchor2.Set(-4, 0);

	}




	distJDef.Initialize(this->aJointearInicial->obtenerCuerpo(),
			this->aJointearFinal->obtenerCuerpo(), anchor1, anchor2);
	distJDef.collideConnected = true;
	distJDef.dampingRatio = 1.0f;
	distJDef.frequencyHz = 0.0f;
	distJDef.localAnchorA = anchor1;
	distJDef.localAnchorB = anchor2;



	b2RopeJointDef rDef;
	if (anchor1.Length() != 0 && anchor2.Length() != 0) {
		//Inicial y final hay balancin
		rDef.maxLength =
				(posicionJointearFinal - posicionJointearInicial).Length();
		cout << "maxLength 1: " << rDef.maxLength << endl;
	} else if (anchor1.Length() != 0) {
		rDef.maxLength = (this->aJointearFinal->obtenerCuerpo()->GetPosition()
				- posicionJointearInicial).Length(); //ACA ESTABA LA FALLA, PORQUE DECIA FINAL, EN VEZ DE INICIAL!!!//
		cout << "maxLength 2: " << rDef.maxLength << endl;
	} else if (anchor2.Length() != 0) {
		rDef.maxLength =
				(posicionJointearFinal
						- this->aJointearInicial->obtenerCuerpo()->GetPosition()).Length();
		cout << "maxLength 31: " << rDef.maxLength << endl;

	} else {
		rDef.maxLength =
				((this->aJointearFinal->obtenerCuerpo()->GetPosition())
						- (this->aJointearInicial->obtenerCuerpo()->GetPosition())).Length();
		cout << "maxLength 4: " << rDef.maxLength << endl;

	}
	rDef.localAnchorA = anchor1;
	rDef.localAnchorB = anchor2;
	rDef.bodyA = this->aJointearInicial->obtenerCuerpo();
	rDef.bodyB = this->aJointearFinal->obtenerCuerpo();



	jointCuerda = world->CreateJoint(&rDef);
	}
}

void SogaFisica::eliminarSoga(b2World& world) {
	if (this->jointCuerda){
		world.DestroyJoint(jointCuerda);
		jointCuerda = NULL;
	}
}

void SogaFisica::destruirEntidadFisica(){
	cout<<"ENTRO A DESTRUIR LA CUERDA"<<endl;
	b2World* world = this->cuerpoFisico->GetWorld();
	eliminarSoga(*world);
	world->DestroyBody(this->cuerpoFisico);
}


SogaFisica::~SogaFisica() {
	// TODO Auto-generated destructor stub
}

